/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/Src/node.cpp
 * @Description  : 该文件实现了节点（Node）和节点基类（NodeBase）的功能，用于管理和操作AGV系统中的节点数据。
 *                 节点类包含了节点的基本属性和方法，例如判断节点类型、获取坐标等。
 *                 节点基类则提供了节点的创建、查找、添加、删除及修改等操作。
 *                 文件中还包含了XML解析的辅助函数，用于从XML配置文件中读取节点信息。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:35:04
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <stdlib.h> 
#include <stdio.h>
#include "node.h"
#include "fstream"
namespace xml_tool
{
    std::string FindMapId(XMLElement *root)
    {

        if (!root)
        {
            DEBUG_ERROR_OUT("FindMapId failed: root is null");
            return "";
        }

        try
        {
            std::string robot_id = std::string(getenv("ROBOT_ID"));
            std::string group_name = "group";
            std::string agv_number_name = "agv_number";
            XMLElement *first_element = root->FirstChildElement(group_name.c_str());

            while (first_element != NULL)
            {
                std::string cur_element_name = std::string(first_element->Name());
                if (group_name == cur_element_name)
                {
                    std::string map_id = first_element->FindAttribute("mapId") ? first_element->FindAttribute("mapId")->Value() : "";
                    XMLElement *first_agv_number = first_element->FirstChildElement(agv_number_name.c_str());
                    while (first_agv_number != NULL)
                    {
                        std::string cur_number_name = std::string(first_agv_number->Name());
                        if (agv_number_name == cur_number_name)
                        {
                            std::string id = first_agv_number->FindAttribute("id") ? first_agv_number->FindAttribute("id")->Value() : "null_agv_id";
                            if (id == robot_id)
                            {
                                DEBUG_OUT("find cur map id ok mapId:" << map_id << ",cur_robot_id:" << robot_id);
                                return map_id;
                            }
                        } // 第二层

                        first_agv_number = first_agv_number->NextSiblingElement();
                    }
                } // 第一层
                first_element = first_element->NextSiblingElement();
            }
        }
        catch (...)
        {
            DEBUG_ERROR_OUT("error: FindMapId crash");
        }

        return "map_id_null";
    }

    XMLElement *FindMapData(XMLElement *root, const std::string &target_map_id)
    {

        if (!root)
        {
            DEBUG_ERROR_OUT("FindMapData failed: root is null");
            return NULL;
        }

        try
        {
            std::string map_name = "map";
            XMLElement *first_element = root->FirstChildElement(map_name.c_str());

            while (first_element != NULL)
            {
                std::string cur_element_name = std::string(first_element->Name());
                if (map_name == cur_element_name)
                {
                    std::string map_id = first_element->FindAttribute("id") ? first_element->FindAttribute("id")->Value() : "";
                    if (target_map_id == map_id)
                    {
                        DEBUG_OUT("find target_map_id ok:target_map_id:" << target_map_id);
                        return first_element;
                    }
                }
                first_element = first_element->NextSiblingElement();
            }
        }
        catch (...)
        {
            DEBUG_ERROR_OUT("error: FindMapData crash");
        }

        return NULL;
    }

}

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CNode".

//
//   CNode: The constructor.
//
Node::Node(unsigned short id, const Point& pt, unsigned short type, float heading)
    : Point(pt), id_(id), type_(type), heading_(heading) 
{
    InitValue();
}

Node::Node() : Point(), id_(0), type_(NORMAL_NODE), heading_(0.0f)
{
    InitValue();
}

void Node::InitValue() {
    chk_mark_dist_ = 0.3f;
    chk_mark_vel_ = 0.1f;
    qr_code_id_ = -1;
    max_spin_value_ = max_spin_load_ = 0.45f;
    spin_acc_value_ = spin_acc_load_ = 0.2f;
    spin_dcc_value_ = spin_dcc_load_ = 0.2f;
    enable_node_angle_ = false;
    enable_pick_angle_ = false;
    enable_drop_angle_ = false;
    enable_spin_ = false;
    enable_load_centering_ = false;
    enable_table_tunning_ = false;
    enable_mark_code_ = false;
    detect_plan_ = 3;
    bits_ = 0;
    word1_ = -1;
    word2_ = -1;
    head_pick_ = 0.0f;
    head_drop_ = 0.0f;
    strcpy(zone_tag_, "defaultZoneTag");
    switch_point_ = false;
}

//
//   IsMarkNode: Check if a mark is available at this node.
//
bool Node::IsMarkNode() const
{
    return ((type_ & MARK_NODE) != 0);
}

//
//   IsChargerNode: Check if this node is a charger station.
//
bool Node::IsChargerNode() const 
{
    return ((type_ & CHARGER_NODE) != 0);
}

//
//   ZoneRotate: Check if this node can rotate to node's angle.
//
bool Node::EnableSpin() const 
{
    return enable_spin_;
}

bool Node::EnableNodeAngle() const 
{
    return enable_node_angle_;
}


bool Node::EnablePickAngle() const 
{
    return enable_pick_angle_;
}

bool Node::EnableDropAngle() const 
{
    return enable_drop_angle_;
}

bool Node::EnableLoadCentering() const 
{
    return enable_load_centering_;
}

bool Node::EnableTableTunning() const 
{
    return enable_table_tunning_;
}

bool Node::EnableMarkCode() const
{
    return enable_mark_code_;
}

uint16_t Node::Bits() const 
{
    return bits_;
}

void Node::Words(int& word1, int& word2) 
{
    word1 = word1_;
    word2 = word2_;
}

int Node::GetDetectPlan() const 
{
    return detect_plan_;
}

void Node::GetActionHead(float& head_pick, float& head_drop) 
{
    head_pick = head_pick_;
    head_drop = head_drop_;
}

void Node::GetCrossFloorProperty(std::string& zone_tag, bool& switch_point) 
{
    zone_tag = std::string(zone_tag_);
    switch_point = switch_point_;
}

//
//   operator "==": Check if 2 nodes are identical.
//
bool Node::operator==(const Node& nd) const 
{
    return (id_ == nd.id_);
}

//
//   operator "!=": Check if 2 nodes are not identical.
//
bool Node::operator!=(const Node& nd) const 
{
    return (id_ != nd.id_);
}

// m_nWord1
//    operator "==": Check if 2 nodes are identical.
//
bool Node::operator==(unsigned short node_id) const 
{
    return (id_ == node_id);
}

//
//   operator "!=": Check if 2 nodes are not identical.
//
bool Node::operator!=(unsigned short node_id) const 
{
    return (id_ != node_id);
}

//
//    Create the node data from a text file.
//
// BOOL CNode::Create()
// {
//     m_uId = 0;
//     m_uType = NORMAL_NODE; // MARK_NODE
//     x = 0.0f;
//     y = 0.0f;
//     m_fHeading = 0.0f;
//     m_fChkMarkDist = 0.3;
//     m_fChkMarkVel = 0.1f;

//     DEBUG_OUT("CNode: m_uId =" << m_uId << ",m_uType=" << m_uType
//                                << "x=" << x << ",y=" << y
//                                << ",m_fChkMarkDist=" << m_fChkMarkDist
//                                << ",m_fChkMarkVel=" << m_fChkMarkVel);
//     return true;
// }

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CNodeBase".                                   *

//
//   The constructor of class "CNodeBase".
//
NodeBase::NodeBase() : count_(0), nodes_(nullptr) {}

//
//   The destructor of class "CNodeBase".
//
NodeBase::~NodeBase() {
    Clear();
}


void NodeBase::Clear() {
    if (nodes_ != nullptr) {
        free(nodes_);
        count_ = 0;
        nodes_ = nullptr;
    }
}

//
//    Get the specified node object. by qrcode
//
// CNode CNodeBase::GetNodeByQrCode(int nQrCode)
// {
//    std::lock_guard<std::mutex> locker(m_ndMutex);

//     // Make sure the node base was created
//     if (m_paNode == NULL)
//         return CNode();

//     for (USHORT i = 0; i < m_uCount; i++)
//     {
//         if (m_paNode[i].m_qrCodeId == nQrCode)
//             return m_paNode[i];
//     }

//     return CNode();
// }

bool NodeBase::FindNearNodeByXy(const Posture& pst, Node& node, float threshold_dis) {
    std::lock_guard<std::mutex> locker(node_mutex_);
    if (nodes_ == nullptr) return false;

    float min_dis = 99999.0f;
    bool found = false;
    for (unsigned short i = 0; i < count_; i++) {
        Node& cur_node = nodes_[i];
        float dx = cur_node.x_ - pst.x_;
        float dy = cur_node.y_ - pst.y_;
        float dist = std::sqrt(dx * dx + dy * dy);
        if (dist < min_dis && dist <= threshold_dis) {
            min_dis = dist;
            node = cur_node;
            found = true;
        }
    }
    if (found) {
        DEBUG_OUT("FindNearNodeByXy OK: minDis=" << min_dis << ", thresholdDis=" << threshold_dis);
    } else {
        DEBUG_OUT("FindNearNodeByXy failed: minDis=" << min_dis << ", thresholdDis=" << threshold_dis);
    }
    return found;
}

//
//    Get the specified node object.
//
Node* NodeBase::GetNode(unsigned short id) {
    std::lock_guard<std::mutex> locker(node_mutex_);
    if (nodes_ == nullptr) {
        DEBUG_ERROR_OUT("error: GetNode id=" << id << ", nodes_ is null");
        return nullptr;
    }
    for (unsigned short i = 0; i < count_; i++) {
        if (nodes_[i].id_ == id) {
            return &nodes_[i];
        }
    }
    DEBUG_ERROR_OUT("error: GetNode cannot find id=" << id << ", count=" << count_);
    return nullptr;
}

//
//   Check if a code is a valid node ID.
//
bool NodeBase::IsNode(unsigned short code) 
{
    return (GetNode(code) != nullptr);
}

//
// Get the pointer to the node data base object
//
NodeBase& NodeBase::GetNodeBaseObject() 
{
    return *this;
}

// Get the X coordinate of the left-most point
float NodeBase::LeftMost() const 
{
    if (count_ == 0) return 0.0f;
    float left_most = nodes_[0].x_;
    for (unsigned short i = 1; i < count_; i++) {
        if (left_most > nodes_[i].x_) {
            left_most = nodes_[i].x_;
        }
    }
    return left_most;
}

// Get the Y coordinate of the top-most point
float NodeBase::TopMost() const
{
    if (count_ == 0) return 0.0f;
    float top_most = nodes_[0].y_;
    for (unsigned short i = 1; i < count_; i++) {
        if (top_most < nodes_[i].y_) {
            top_most = nodes_[i].y_;
        }
    }
    return top_most;
}

// Get the X coordinate of the right-most point
float NodeBase::RightMost() const 
{
    if (count_ == 0) return 0.0f;
    float right_most = nodes_[0].x_;
    for (unsigned short i = 1; i < count_; i++) {
        if (right_most < nodes_[i].x_) {
            right_most = nodes_[i].x_;
        }
    }
    return right_most;
}

// Get the Y coordinate of the bottom-most point
float NodeBase::BottomMost() const 
{
    if (count_ == 0) return 0.0f;
    float bottom_most = nodes_[0].y_;
    for (unsigned short i = 1; i < count_; i++) {
        if (bottom_most > nodes_[i].y_) {
            bottom_most = nodes_[i].y_;
        }
    }
    return bottom_most;
}

//
//   Get the width of the map area.
//
float NodeBase::Width() const 
{
    return RightMost() - LeftMost();
}

//
//   Get the height of the map area.
//
float NodeBase::Height() const 
{
    return TopMost() - BottomMost();
}

//
//   Get the coordinates of the center point.
//
Point NodeBase::Center() const 
{
    Point pt;
    pt.x_ = (RightMost() + LeftMost()) / 2;
    pt.y_ = (TopMost() + BottomMost()) / 2;
    return pt;
}

//
//   Create nodes bank data from a text file.
//
bool NodeBase::Create(const std::string& map_path) 
{
    std::lock_guard<std::mutex> locker(node_mutex_);
    Clear();
    tinyxml2::XMLDocument doc;
    if (doc.LoadFile(map_path.c_str()) != 0) {
        DEBUG_OUT("load xml file failed");
        return false;
    }
    tinyxml2::XMLElement* root = doc.RootElement();
    if (!root) {
        DEBUG_OUT("xml root is nullptr");
        return false;
    }
    std::string map_id = xml_tool::FindMapId(root);
    tinyxml2::XMLElement* second_root = xml_tool::FindMapData(root, map_id);
    map_id_ = map_id;
    if (!second_root) {
        DEBUG_OUT("xml second_root is nullptr");
        return false;
    }
    std::string point_element_name = "point";
    tinyxml2::XMLElement* first_point_node = second_root->FirstChildElement(point_element_name.c_str());
    std::vector<Node> node_vector;
    while (first_point_node != nullptr) {
        std::string cur_node_name = std::string(first_point_node->Name());
        if (point_element_name == cur_node_name) {
            Node node;
            auto p_temp = first_point_node->FindAttribute("id");
            node.id_ = p_temp ? p_temp->IntValue() : 65535;
            p_temp = first_point_node->FindAttribute("type");
            node.type_ = p_temp ? p_temp->IntValue() : 0;
            p_temp = first_point_node->FindAttribute("x");
            node.x_ = p_temp ? p_temp->FloatValue() : 0.0f;
            p_temp = first_point_node->FindAttribute("y");
            node.y_ = p_temp ? p_temp->FloatValue() : 0.0f;
            p_temp = first_point_node->FindAttribute("angle");
            node.heading_ = p_temp ? p_temp->FloatValue() : 0.0f;
            p_temp = first_point_node->FindAttribute("qrCodeId");
            node.qr_code_id_ = p_temp ? p_temp->IntValue() : -1;
            p_temp = first_point_node->FindAttribute("avoidType");
            node.detect_plan_ = p_temp ? p_temp->IntValue() : 3;
            p_temp = first_point_node->FindAttribute("maxSpinVel");
            node.max_spin_value_ = p_temp ? p_temp->FloatValue() : 0.4f;
            p_temp = first_point_node->FindAttribute("maxSpinVelLoad");
            node.max_spin_load_ = p_temp ? p_temp->FloatValue() : node.max_spin_value_;
            p_temp = first_point_node->FindAttribute("spinAccVel");
            node.spin_acc_value_ = p_temp ? p_temp->FloatValue() : 0.2f;
            p_temp = first_point_node->FindAttribute("spinAccVelLoad");
            node.spin_acc_load_ = p_temp ? p_temp->FloatValue() : node.spin_acc_value_;
            p_temp = first_point_node->FindAttribute("spinDccVel");
            node.spin_dcc_value_ = p_temp ? p_temp->FloatValue() : 0.2f;
            p_temp = first_point_node->FindAttribute("spinDccVelLoad");
            node.spin_dcc_load_ = p_temp ? p_temp->FloatValue() : node.spin_dcc_value_;
            p_temp = first_point_node->FindAttribute("enableSpin");
            node.enable_spin_ = p_temp ? p_temp->BoolValue() : 0;
            p_temp = first_point_node->FindAttribute("enableNodeAngle");
            node.enable_node_angle_ = p_temp ? p_temp->BoolValue() : 0;
            p_temp = first_point_node->FindAttribute("enablePickAngle");
            node.enable_pick_angle_ = p_temp ? p_temp->BoolValue() : 0;
            p_temp = first_point_node->FindAttribute("enableDropAngle");
            node.enable_drop_angle_ = p_temp ? p_temp->BoolValue() : 0;
            p_temp = first_point_node->FindAttribute("enableLoadCentering");
            node.enable_load_centering_ = p_temp ? p_temp->BoolValue() : 0;
            p_temp = first_point_node->FindAttribute("enableTableTunning");
            node.enable_table_tunning_ = p_temp ? p_temp->BoolValue() : 0;
            p_temp = first_point_node->FindAttribute("landMarkCheck");
            node.enable_mark_code_ = p_temp ? p_temp->BoolValue() : 0;
            p_temp = first_point_node->FindAttribute("bits");
            node.bits_ = p_temp ? p_temp->IntValue() : 0;
            p_temp = first_point_node->FindAttribute("qrOffsetX");
            node.qr_offset_.qr_offset_x = p_temp ? p_temp->FloatValue() : 0.0f;
            p_temp = first_point_node->FindAttribute("qrOffsetY");
            node.qr_offset_.qr_offset_y = p_temp ? p_temp->FloatValue() : 0.0f;
            p_temp = first_point_node->FindAttribute("qrOffsetAngle");
            node.qr_offset_.qr_offset_angle = p_temp ? p_temp->FloatValue() : 0.0f;
            p_temp = first_point_node->FindAttribute("word1");
            node.word1_ = p_temp ? p_temp->IntValue() : -1;
            p_temp = first_point_node->FindAttribute("word2");
            node.word2_ = p_temp ? p_temp->IntValue() : -1;
            p_temp = first_point_node->FindAttribute("headPick");
            node.head_pick_ = p_temp ? p_temp->FloatValue() : 0.0f;
            p_temp = first_point_node->FindAttribute("headDrop");
            node.head_drop_ = p_temp ? p_temp->FloatValue() : 0.0f;
            p_temp = first_point_node->FindAttribute("zoneTag");
            std::string tag = p_temp ? p_temp->Value() : "UnknownZoneTag";
            if (tag.size() > MAX_ZONE_NAME_BYTES - 1) {
                tag = "tag too long";
                DEBUG_ERROR_OUT("error: zoneTag is too long");
            }
            strcpy(node.zone_tag_, tag.c_str());
            p_temp = first_point_node->FindAttribute("isSwitchPoint");
            node.switch_point_ = p_temp ? p_temp->BoolValue() : 0;
            node_vector.push_back(node);
        }
        first_point_node = first_point_node->NextSiblingElement();
    }
    count_ = node_vector.size();
    nodes_ = static_cast<Node*>(calloc(count_, sizeof(Node)));
    if (nodes_ == nullptr) return false;
    DEBUG_OUT("point count=" << count_);
    for (unsigned short i = 0; i < count_; i++) {
        nodes_[i] = node_vector.at(i);
        DEBUG_OUT("i=" << i
                       << " ,id=" << nodes_[i].id_
                       << " ,type=" << nodes_[i].type_
                       << " ,x=" << nodes_[i].x_
                       << " ,y=" << nodes_[i].y_
                       << " ,headAngle=" << nodes_[i].heading_
                       << ",qrCodeId=" << nodes_[i].qr_code_id_
                       << ",maxSpinVel=" << nodes_[i].max_spin_value_
                       << ",spinAccVel=" << nodes_[i].spin_acc_value_
                       << ",spinDccVel=" << nodes_[i].spin_dcc_value_
                       << ",maxSpinVelLoad=" << nodes_[i].max_spin_load_
                       << ",spinAccVelLoad=" << nodes_[i].spin_acc_load_
                       << ",spinDccVelLoad=" << nodes_[i].spin_dcc_load_
                       << ",enableSpin=" << nodes_[i].enable_spin_
                       << ",enableNodeAngle=" << nodes_[i].enable_node_angle_
                       << ",enablePickAngle=" << nodes_[i].enable_pick_angle_
                       << ",enableDropAngle=" << nodes_[i].enable_drop_angle_
                       << ",enableLoadCentering=" << nodes_[i].enable_load_centering_
                       << ",enableTableTunning=" << nodes_[i].enable_table_tunning_
                       << ",enableMarkCode=" << nodes_[i].enable_mark_code_
                       << ",bits=" << nodes_[i].bits_
                       << ",qrOffsetX=" << nodes_[i].qr_offset_.qr_offset_x
                       << ",qrOffsetY=" << nodes_[i].qr_offset_.qr_offset_y
                       << ",qrOffsetAngle=" << nodes_[i].qr_offset_.qr_offset_angle
                       << ",word1=" << nodes_[i].word1_
                       << ",word2=" << nodes_[i].word2_
                       << ",headPick=" << nodes_[i].head_pick_
                       << ",headDrop=" << nodes_[i].head_drop_
                       << ",zoneTag=" << std::string(nodes_[i].zone_tag_)
                       << ",switchPoint=" << nodes_[i].switch_point_);
    }
    return true;
}

//
//   Add a node to the nodes data base.
//   Return:
//     0 - Success
//    -1 - Out of memory
//    -2 - A node with the same ID already exist in the data base
//
short NodeBase::AddNode(Node& nd) 
{
    if (GetNode(nd.id_) != nullptr) return -2;
    Node* temp = static_cast<Node*>(calloc(count_ + 1, sizeof(Node)));
    if (temp == nullptr) return -1;
    for (unsigned short i = 0; i < count_; i++) {
        temp[i] = nodes_[i];
    }
    temp[count_] = nd;
    Clear();
    nodes_ = temp;
    count_++;
    return 0;
}

//
//   Delete a node from the nodes data base.
//   Return:
//     0 - Success
//    -1 - Out of memory
//    -2 - Such node does not exist in the data base
//
short NodeBase::RemoveNode(unsigned short id) 
{
    if (GetNode(id) == nullptr) return -2;
    Node* temp = static_cast<Node*>(calloc(count_ - 1, sizeof(Node)));
    if (temp == nullptr) return -1;
    unsigned short j = 0;
    for (unsigned short i = 0; i < count_; i++) {
        if (nodes_[i].id_ != id) {
            temp[j++] = nodes_[i];
        }
    }
    Clear();
    nodes_ = temp;
    count_--;
    return 0;
}

//
//   Modify the ID of a node.
//   Return:
//      0 - Success
//     -1 - The Specified old node does not exist in the database
//     -2 - The specified new ID already exists in the database
//
short NodeBase::ModifyNodeID(unsigned short old_id, unsigned short new_id) 
{
    Node* node = GetNode(old_id);
    if (node == nullptr) return -1;
    if (GetNode(new_id) != nullptr) return -2;
    node->id_ = new_id;
    return 0;
}

//
//   Modify the type of a node.
//
bool NodeBase::ModifyNodeType(unsigned short id, unsigned short new_type) 
{
    Node* node = GetNode(id);
    if (node == nullptr) return false;
    node->type_ = new_type;
    return true;
}

//
//   Modify the extended type of a node.
//
// BOOL CNodeBase::ModifyNodeExtType(USHORT uId, SHORT uNewExtType)
// {
//     CNode *pNode = GetNode(uId);
//     if (pNode == NULL)
//         return FALSE;

//     pNode->m_uExtType = uNewExtType;
//     return TRUE;
// }

//
//   Modify the location of a node.
//
bool NodeBase::ModifyNodePoint(unsigned short id, float x, float y) 
{
    Node* node = GetNode(id);
    if (node == nullptr) return false;
    node->x_ = x;
    node->y_ = y;
    return true;
}
